#!/usr/bin/env python3

# 运行参数管理示例
# # 终端 1: 启动参数管理节点
# python3 param_manager.py
#
# # 终端 2: 使用 ROS2 命令行工具操作参数
# ros2 param list /parameter_manager
# ros2 param get /parameter_manager robot_name
# ros2 param set /parameter_manager robot_name "我的机器人"
# ros2 param set /parameter_manager max_speed 150
# ros2 param get /parameter_manager max_speed
# ros2 param set /parameter_manager debug_mode true

import rclpy
from rclpy.node import Node
from rclpy.parameter import Parameter
from rcl_interfaces.msg import ParameterDescriptor, ParameterType
import time


class ParameterManager(Node):
    def __init__(self):
        super().__init__('parameter_manager')

        # 声明参数并设置默认值和描述
        self.declare_parameters(
            namespace='',
            parameters=[
                # 字符串参数
                ('robot_name', 'my_robot',
                 ParameterDescriptor(description='机器人名称')),

                # 整数参数
                ('max_speed', 100,
                 ParameterDescriptor(description='最大速度 (cm/s)', type=ParameterType.PARAMETER_INTEGER)),

                # 浮点参数
                ('turning_radius', 0.5,
                 ParameterDescriptor(description='转弯半径 (m)', type=ParameterType.PARAMETER_DOUBLE)),

                # 布尔参数
                ('debug_mode', False,
                 ParameterDescriptor(description='调试模式开关', type=ParameterType.PARAMETER_BOOL)),

                # 数组参数
                ('sensor_positions', [0.1, 0.2, 0.3],
                 ParameterDescriptor(description='传感器位置列表', type=ParameterType.PARAMETER_DOUBLE_ARRAY))
            ]
        )

        # 设置参数变化回调
        self.add_on_set_parameters_callback(self.parameter_change_callback)

        # 创建定时器，定期显示参数值
        self.timer = self.create_timer(2.0, self.display_parameters)

        self.get_logger().info('Parameter Manager 已启动')
        self.get_logger().info('可以使用以下命令修改参数:')
        self.get_logger().info('ros2 param set /parameter_manager robot_name "新名字"')
        self.get_logger().info('ros2 param set /parameter_manager max_speed 150')

    def parameter_change_callback(self, params):
        """参数变化回调函数"""
        for param in params:
            self.get_logger().info(
                f'参数 "{param.name}" 已更改: {param.value} (类型: {param.type_})'
            )

            # 根据参数变化执行特定操作
            if param.name == 'debug_mode':
                if param.value:
                    self.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG)
                    self.get_logger().info('调试模式已开启')
                else:
                    self.get_logger().set_level(rclpy.logging.LoggingSeverity.INFO)
                    self.get_logger().info('调试模式已关闭')

            elif param.name == 'max_speed':
                if param.value > 200:
                    self.get_logger().warn(f'警告: 最大速度 {param.value} 可能过高!')

        # 返回成功结果
        from rcl_interfaces.msg import SetParametersResult
        return SetParametersResult(successful=True)

    def display_parameters(self):
        """定期显示当前参数值"""
        # 获取所有参数
        robot_name = self.get_parameter('robot_name').value
        max_speed = self.get_parameter('max_speed').value
        turning_radius = self.get_parameter('turning_radius').value
        debug_mode = self.get_parameter('debug_mode').value
        sensor_positions = self.get_parameter('sensor_positions').value

        self.get_logger().info('=== 当前参数值 ===')
        self.get_logger().info(f'机器人名称: {robot_name}')
        self.get_logger().info(f'最大速度: {max_speed} cm/s')
        self.get_logger().info(f'转弯半径: {turning_radius} m')
        self.get_logger().info(f'调试模式: {debug_mode}')
        self.get_logger().info(f'传感器位置: {sensor_positions}')

        # 演示参数的实际使用
        if debug_mode:
            self.get_logger().debug('这是调试信息 - 只在调试模式下显示')

    def get_robot_config(self):
        """获取机器人配置的便捷方法"""
        return {
            'name': self.get_parameter('robot_name').value,
            'max_speed': self.get_parameter('max_speed').value,
            'turning_radius': self.get_parameter('turning_radius').value,
            'debug_mode': self.get_parameter('debug_mode').value,
            'sensor_positions': self.get_parameter('sensor_positions').value
        }


def main(args=None):
    rclpy.init(args=args)

    node = ParameterManager()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()